#include "Matrix33.h"

Matrix33::Matrix33(Quaternion &rotQuat)
{
	rotationFromQuat(rotQuat);
}

Matrix33::Matrix33(float scaleX, float scaleY, float scaleZ)
{
	mat[0] = scaleX;
	mat[4] = scaleY;
	mat[8] = scaleZ;
	
	mat[1] = mat[2] = mat[3] = mat[5] = mat[6] = mat[7] = 0;
}

Matrix33::Matrix33(float scale)
{
	mat[0] = mat[4] = mat[8] = scale;
	mat[1] = mat[2] = mat[3] = mat[5] = mat[6] = mat[7] = 0;
}

Matrix33::Matrix33(Matrix33 &otherMatrix)
{
	
}

void Matrix33::rotationFromQuat(Quaternion &rotQuat)
{
//Taken from Essential Mathematics and Physics for Games
//Makes a lot of sense actually, only does multiplications where it absolutely
//needs to.

	float x_y, x_z, y_z, w_x, w_y, w_z, x_x, y_y, z_z;
	float scale;
	float scaleX, scaleY, scaleZ;
	
	scale = 2.0 / rotQuat.w * rotQuat.w + rotQuat.x * rotQuat.x + rotQuat.y * rotQuat.y + rotQuat.z * rotQuat.z;
	
	scaleX = scale * rotQuat.x;
	scaleY = scale * rotQuat.y;
	scaleZ = scale * rotQuat.z;
	
	w_x = rotQuat.w * scaleX;
	w_y = rotQuat.w * scaleY;
	w_z = rotQuat.w * scaleZ;
	
	x_x = rotQuat.x * scaleX;
	x_y = rotQuat.x * scaleY;
	x_z = rotQuat.x * scaleZ;
	
	y_y = rotQuat.y * scaleY;
	y_z = rotQuat.y * scaleZ;
	
	z_z = rotQuat.z * scaleZ;

	//Check out pg 189 in the essential math game.
	//We got rid of the 2's bc we already multiplied that in with the scale.
	
	mat[0] = 1.0 - y_y - z_z;
	mat[1] = x_y + w_z;
	mat[2] = x_z - w_y;
	mat[3] = x_y - w_z;
	mat[4] = 1.0 - x_x - z_z;
	mat[5] = y_z + w_x;
	mat[6] = x_z + w_y;
	mat[7] = y_z - w_x;
	mat[8] = 1 - x_x - y_y;
	
}

Matrix33 Matrix33::operator*(Matrix33 &otherMatrix)
{
	Matrix33 temp;
	
	temp.mat[0] = mat[0] * otherMatrix.mat[0] + mat[3] * otherMatrix.mat[1] + mat[6] * otherMatrix.mat[2];
	temp.mat[1] = mat[1] * otherMatrix.mat[0] + mat[4] * otherMatrix.mat[1] + mat[7] * otherMatrix.mat[2];
	temp.mat[2] = mat[2] * otherMatrix.mat[0] + mat[5] * otherMatrix.mat[1] + mat[8] * otherMatrix.mat[2];
	
	temp.mat[3] = mat[0] * otherMatrix.mat[3] + mat[3] * otherMatrix.mat[4] + mat[6] * otherMatrix.mat[5];
	temp.mat[4] = mat[1] * otherMatrix.mat[3] + mat[4] * otherMatrix.mat[4] + mat[7] * otherMatrix.mat[5];
	temp.mat[5] = mat[2] * otherMatrix.mat[3] + mat[5] * otherMatrix.mat[4] + mat[8] * otherMatrix.mat[5];
	
	temp.mat[6] = mat[0] * otherMatrix.mat[6] + mat[3] * otherMatrix.mat[7] + mat[6] * otherMatrix.mat[8];
	temp.mat[7] = mat[1] * otherMatrix.mat[6] + mat[4] * otherMatrix.mat[7] + mat[7] * otherMatrix.mat[8];
	temp.mat[8] = mat[2] * otherMatrix.mat[6] + mat[5] * otherMatrix.mat[7] + mat[8] * otherMatrix.mat[8];
	
	return temp;	
}

Vector3f Matrix33::operator* (Vector3f &otherVector)
{
	return Vector3f(mat[0] * otherVector.x + mat[3] * otherVector.y + mat[6] * otherVector.z, mat[1] * otherVector.x + mat[4] * otherVector.y + mat[7] * otherVector.z, mat[2] * otherVector.x + mat[5] * otherVector.y + mat[8] * otherVector.z);
}

void Matrix33::operator*= (Matrix33 &otherMatrix)
{
	mat[0] = mat[0] * otherMatrix.mat[0] + mat[3] * otherMatrix.mat[1] + mat[6] * otherMatrix.mat[2];
	mat[1] = mat[1] * otherMatrix.mat[0] + mat[4] * otherMatrix.mat[1] + mat[7] * otherMatrix.mat[2];
	mat[2] = mat[2] * otherMatrix.mat[0] + mat[5] * otherMatrix.mat[1] + mat[8] * otherMatrix.mat[2];
	
	mat[3] = mat[0] * otherMatrix.mat[3] + mat[3] * otherMatrix.mat[4] + mat[6] * otherMatrix.mat[5];
	mat[4] = mat[1] * otherMatrix.mat[3] + mat[4] * otherMatrix.mat[4] + mat[7] * otherMatrix.mat[5];
	mat[5] = mat[2] * otherMatrix.mat[3] + mat[5] * otherMatrix.mat[4] + mat[8] * otherMatrix.mat[5];
	
	mat[6] = mat[0] * otherMatrix.mat[6] + mat[3] * otherMatrix.mat[7] + mat[6] * otherMatrix.mat[8];
	mat[7] = mat[1] * otherMatrix.mat[6] + mat[4] * otherMatrix.mat[7] + mat[7] * otherMatrix.mat[8];
	mat[8] = mat[2] * otherMatrix.mat[6] + mat[5] * otherMatrix.mat[7] + mat[8] * otherMatrix.mat[8];
}

Matrix33 Matrix33::operator+ (Matrix33 &otherMatrix)
{
	Matrix33 temp;
	
	for (unsigned int i = 0; i < 9; i++)
	{
		temp.mat[i] = mat[i] + otherMatrix.mat[i];
	}
	
	return temp;
}
void Matrix33::operator+= (Matrix33 &otherMatrix)
{
	for (unsigned int i = 0; i < 9; i++)
	{
		mat[i] += otherMatrix.mat[i];
	}
}

Matrix33 Matrix33::operator- (Matrix33 &otherMatrix)
{
	Matrix33 temp;
	
	for (unsigned int i = 0; i < 9; i++)
	{
		temp.mat[i] = mat[i] - otherMatrix.mat[i];
	}
	
	return temp;
}
void Matrix33::operator-= (Matrix33 &otherMatrix)
{	
	for (unsigned int i = 0; i < 9; i++)
	{
		mat[i] -= otherMatrix.mat[i];
	}
}
